/**
 * Snowflake MEGA sumo robot strategy nr.5 - retrieve backwards to the line and wait for opponent to attack
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "strategy5.h"
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"


static void my_callback(void)
{
	// User code to execute when the overflow occurs here
	Speed=100;
	//tc_disable(&TCC0);	//not needed any more
}


void strategy5_app(void)
{
	struct keyboard_event input;

	// set speed for the strategy
	Speed=0;	// global volatile variable
	int8_t speed = 100;
	bool first_move_dir_left = true;

		tc_enable(&TCC0);	// 16-bit => max=65536
		tc_set_overflow_interrupt_callback(&TCC0, my_callback);
		tc_set_wgm(&TCC0, TC_WG_NORMAL);
		tc_write_period(&TCC0, 62500);	// 0.5Hz => 2sec delay at the line
		tc_set_overflow_interrupt_level(&TCC0, TC_INT_LVL_LO);
		//cpu_irq_enable(); // already done in main
		tc_write_clock_source(&TCC0, TC_CLKSEL_DIV1024_gc);	// 32MHz/1024= 31250 Hz

	// show select info
	gfx_mono_draw_string("Drive backward & then:", 0, 14, &sysfont);
	gfx_mono_draw_string("ENTER-Left,  UP-Right", 0, 24, &sysfont);

	// Set the PWM
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			if (input.keycode == KEYBOARD_ENTER) {
				first_move_dir_left = true;
				break;
			} else if (input.keycode == KEYBOARD_UP) {
				first_move_dir_left = false;
				break;
			} else if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				// go back to FIGHT menu
				return;
			}
		}
	}

	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	if (first_move_dir_left) {
		// show selected move info
		gfx_mono_draw_string("Strategy 5   drive-BL", 0, 0, &sysfont);
		gfx_mono_draw_string(" Waiting for IR start", 0, 24, &sysfont);
	} else {
		// show selected move info
		gfx_mono_draw_string("Strategy 5   drive-BR", 0, 0, &sysfont);
		gfx_mono_draw_string(" Waiting for IR start", 0, 24, &sysfont);
	}

	while (true) {
		// wait for IR remote start signal
		if ( HIGH == ioport_get_pin_level(REMOTE_START) ){
			break;
		}
	}

	// drop the flag
	turn_servo(true);

	// turn on power for sensors
	sensorPower(true);

	// robot better turns left direction so right turn time needs to be bigger
	if (first_move_dir_left) {
		// turn right
		drive( speed, -speed );
		delay_ms(60); // <========== Calibrate this value!!! ===========
	} else {
		// turn left
		drive( -speed, speed );
		delay_ms(50); // <========== Calibrate this value!!! ===========
	}

	while (true) {
		Line_sens = line_read();
		drive( -speed, -speed ); // <========== Calibrate this value if necessary !!! ===========
		// check kill switch state
		if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
			drive(0,0);
			break;
		}
		if ( (Line_sens&0b00000001) || (Line_sens&0b00001000) ){		// Line_BR or Line_BL
			delay_us(500);
			Line_sens = line_read();
			if ( (Line_sens&0b00000001) || (Line_sens&0b00001000) ){	// Line_BR or Line_BL
				break;
			}
		}
	}

	drive(0,0);	// stop/break
	delay_ms(50);
	drive(100,100);	// drive little forward
	delay_ms(35);	// <========== Calibrate this value if necessary !!! ===========
	tc_reset(&TCC0);
	Speed=0;


	while (true)
	{
		// check kill switch state
		if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
			// kill switch has been activated
			drive(0,0);
			// set IR module back to OFF state
			ioport_set_pin_high(START_SW);
			// turn off power for sensors
			sensorPower(false);
			pwm_start( &pwm_cfg[1], 0 ); // free the servo
			// clear screen
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			return;
		}

		// while loop sensor read delay
		delay_ms(3);

		// read all sensors
		Line_sens = line_read();
		Distance_sens = sensor_read();

		// drive straight with right motor correction !!!
		drive( Speed, Speed*0.9 );


		/*
		 * ######################### Check line sensor state #########################
		 */
		if( (Line_sens&0b00000010) || (Line_sens&0b00000100) ){	// Line_FL or Line_FR
			delay_us(500);
			Line_sens = line_read();
			// Line_FL
			if (Line_sens&0b00000010) {
				drive(0,0);	// stop
				delay_ms(50);
				// turn right while NOT see line again
				while ( !(Line_sens&0b00000001) ){	// Line_BR
					Line_sens = line_read();
					drive(speed,-speed);
					// check kill switch state
					if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
						drive(0,0);
						break;
					}
				}
			// Line_FR
			} else if (Line_sens&0b00000100) {
				drive(0,0);	// stop
				delay_ms(50);
				// turn left while NOT see line again
				while ( !(Line_sens&0b00001000) ){	// Line_BL
					Line_sens = line_read();
					drive(-speed,speed);
					// check kill switch state
					if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
						drive(0,0);
						break;
					}
				}
			}
		}


		/*
		 * ######################### Check distance sensor state #########################
		 */
		if (Distance_sens&0b00010000) {		// L10
			drive( Speed*0.7, speed );	// drive little left
			delay_ms(5);

		} else if (Distance_sens&0b00001000) {	// R10
			drive( speed, Speed*0.7 );	// drive little right
			delay_ms(5);

		} else if (Distance_sens&0b00100000) {	// L30
			drive( Speed/3, Speed );	// drive more left
			delay_ms(5);

		} else if (Distance_sens&0b00000100) {	// R30
			drive( Speed, Speed/3 );	// drive more right
			delay_ms(5);

		} else if (Distance_sens&0b01000000) {	// L45
			drive( -speed, speed );	// turn on spot left
			delay_ms(5);

		} else if (Distance_sens&0b00000010) {	// R45
			drive( speed, -speed );	// turn on spot right
			delay_ms(5);

		} else if (Distance_sens&0b10000000) {	// L90
			drive( -speed, speed );	// turn on spot left
			delay_ms(5);

		} else if (Distance_sens&0b00000001) {	// R90
			drive( speed, -speed );	// turn on spot right
			delay_ms(5);

		} else if ( HIGH == ioport_get_pin_level(PEPPERL_FUCHS) ){
			drive( speed, speed*0.9 );	// drive straight full speed, not sure if necessary !!!
			delay_ms(5);
		}


	}
}